//========================================================================
//References:
//	CMU IPC library reference
//
//Author:
//	Chia-Hsien Louis Chen
//	National Taiwan University of Science and Technology
//	12/2010
//========================================================================
//Syntax:
//	myIPC_Init(centralhost);    -----centralhost is the IP address of the central server
//	Sleep(200);					-----pause time
//
//Description:
//	Initiates the connection to the central server program
//
//Output:
//	No return value
//	command window shows error message of the IPC library(IPC_Ok if everything is all right)
//========================================================================
//Syntax:
//	myShutdown();				-----no arguments
//
//Description:
//	Terminate the connection to the central server, and free allocated resources
//
//Output:
//	No return value
//	command window shows error message of the IPC library(IPC_Ok if everything is all right)
//========================================================================
//Syntax:
//	myGetRobotPose(pose);		-----Returns the most recent pose measurement to a variable pose
//	Sleep(200);					-----pause time
//
//Description:
//	Returns the most recent pose of P3DX with respect to the origin of the 
//	global coordinate system (center of the image). 
//	The pose [x, y, theta] has the units [mm, mm, rad].
//
//	Please note: This function returns the angele theta in rad.
//	
//	The orientation of the global coordinate system is  ---> x
//														|
//														y
//Output:
//	Pose of the P3DX robot.
//	pose [x, y, theta] in [mm, mm, deg]
//========================================================================
//Syntax:
//	Sleep(1000);				-----pause time
//	myGetObstacleCoord();		-----no arguments, Returns coordinates of obstacle points
//	Sleep(50);					-----pause time
//
//Description:
//	Returns a matrix of points which belong to an obstacle. Each row 
//	contains the coordinates of one point [x,y] with respect the origin 
//	of the global coordinate system (center of the image).
//
//Output:
//	Points that belong to obstacles.
//	obstacles =  	[x1, y2]
//					[x2, y2]
//					[. , . ], units [mm, mm].
//========================================================================

//#include "stdafx.h"
//#include "cv.h"
//#include "highgui.h"
#include <stdio.h>
#include "ipc.h"

#include "IPC_CTRL.h"

/* Global variables */
IPC_RETURN_TYPE errMsg;
POSE_DATA_TYPE P3DXPose = {0.0, 0.0, 0.0};
OBSTACLE_DATA_TYPE ObstaclePosData;

/* Mutex variables */
int poseLocked = 0;
int obstaclesLocked = 0;
 static void poseMsgHandler (MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
//void poseMsgHandler (MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{
	static POSE_DATA_TYPE poseData;

	//printf("poseMsgHandler - before unmarshallData\n");
	IPC_unmarshallData(IPC_msgInstanceFormatter(msgRef), callData,
		&poseData, sizeof(poseData));
	//printf("poseMsgHandler - after unmarshallData\n");
	//printf("PoseData: [%.0f %.0f %.3f]\n", poseData.x, poseData.y, poseData.theta);


	while (poseLocked)
	{
		Sleep(1);
	}// mutex unlocked

	/* Lock mutex */
	poseLocked = 1;

	/* Copy pose data */
	P3DXPose.x = poseData.x;
	P3DXPose.y = poseData.y;
	P3DXPose.theta = poseData.theta;

	/* Free mutex */
	poseLocked = 0;

	IPC_freeDataElements(IPC_msgInstanceFormatter(msgRef), &poseData);
	IPC_freeByteArray(callData);
}

static void obstacleMsgHandler (MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
//void obstacleMsgHandler (MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{
	static int i,m,n;
	static OBSTACLE_DATA_TYPE obstacleData;

	IPC_unmarshallData(IPC_msgInstanceFormatter(msgRef), callData,
		&obstacleData, sizeof(obstacleData));

	while (obstaclesLocked)
	{
		Sleep(1);
	}// mutex unlocked

	/* Lock mutex */
	obstaclesLocked = 1;

	/* Copy obstacle data */
	m = ObstaclePosData.dim1 = obstacleData.dim1;
	n = ObstaclePosData.dim2 = obstacleData.dim2;

	printf("[m,n] = [%d %d]\n", m,n);
	//printf("m*n = %d\n", m*n);

	/* Allocate Memory */
	ObstaclePosData.vals = (int*)malloc(m*n*sizeof(int));

	for (i=0; i<m*n; i++)
	{
		ObstaclePosData.vals[i] = obstacleData.vals[i];
		//if (obstacleData.vals[i] == 0)
		//	printf("\nis zero!\n");
	}	

	/* Free mutex */
	obstaclesLocked = 0;

	//for (i=0; i<10; i++)
	//	printf("%d ",ObstaclePosData.vals[i]);
	//printf("\n");

	IPC_freeDataElements(IPC_msgInstanceFormatter(msgRef), &obstacleData);
	IPC_freeByteArray(callData);
}

 static void printErrMsg( const char *msg, IPC_RETURN_TYPE errMsg )
//void printErrMsg( const char *msg, IPC_RETURN_TYPE errMsg )
{
	switch ( errMsg ){
		case 0:
			printf("*** %s \tIPC_Error\n", msg);
			break;
		case 1:
			printf("*** %s \tIPC_Ok\n", msg);
			break;
		case 2:
			printf("*** %s \tIPC_Timeout\n", msg);
			break;
	}
	//if ( errMsg != 1)
	//	IPC_perror("");
}


   void myGetRobotPose( double *out )
//void myGetRobotPose( double *out )
{
	//printf("myGetRobotPose - before listenWait\n");
	//IPC_listenWait( UPDATE_TIME );
	//IPC_listen( UPDATE_TIME );
	IPC_listenClear( UPDATE_TIME );

	//printf("PoseData my: [%.0f %.0f %.3f]\n", P3DXPose.x, P3DXPose.y, P3DXPose.theta);
	//printf("myGetRobotPose - before listenWait");
	while (poseLocked)
	{
		Sleep(10);
	}// mutex unlocked

	/* Lock mutex */
	poseLocked = 1;

	/* Copy pose data */
	out[0] = P3DXPose.x;
	out[1] = P3DXPose.y;
	out[2] = P3DXPose.theta;

	
	//out[i] = data[i]
	//*(out+i) = data[i]

	/* Free mutex */
	poseLocked = 0;

	//printf("getPose PoseData: [%.0f %.0f %.3f]\n", P3DXPose.x, P3DXPose.y, P3DXPose.theta);

}

   static void myGetObstacleCoord(int* out, int m, int n)
	   //void myGetObstacleCoord()
   {
	   static int i,j;
	   //int *out;
	   //IPC_listenWait( UPDATE_TIME_OBSTACLE );
	   //IPC_listen( UPDATE_TIME_OBSTACLE );
	   IPC_listenClear( UPDATE_TIME_OBSTACLE );

	   //printf("myGetObstacleCoord - in \n");

	   while (obstaclesLocked)
	   {
		   Sleep(10);
	   }// mutex unlocked

	   /* Lock mutex */
	   obstaclesLocked = 1;

	   m = ObstaclePosData.dim1;
	   n = ObstaclePosData.dim2;

	   out=(int*)malloc(m*n*sizeof(int));

	   for (i=0; i<m*n; i++)	
	   {
		   out[i] = (int)ObstaclePosData.vals[i];
	   }

	   for (i=0; i<m*n; i++)
	   {
		   if(i%2==0)printf("%5d",out[i]);
		   else printf(" %5d ",out[i]);
	   }

	   /* Free mutex */
	   obstaclesLocked = 0;
	   /* Free global memory */
	   //free( ObstaclePosData.vals );
	   //free(out);
   }



 static void myGetObstacleCoord()
//void myGetObstacleCoord()
{
	static int i,j,m,n;
	int *out;
	//IPC_listenWait( UPDATE_TIME_OBSTACLE );
	//IPC_listen( UPDATE_TIME_OBSTACLE );
	IPC_listenClear( UPDATE_TIME_OBSTACLE );

	//printf("myGetObstacleCoord - in \n");

	while (obstaclesLocked)
	{
		Sleep(10);
	}// mutex unlocked

	/* Lock mutex */
	obstaclesLocked = 1;

	m = ObstaclePosData.dim1;
	n = ObstaclePosData.dim2;

	out=(int*)malloc(m*n*sizeof(int));

	for (i=0; i<m*n; i++)	
	{
		out[i] = (int)ObstaclePosData.vals[i];
	}

	for (i=0; i<m*n; i++)
	{
		if(i%2==0)printf("%5d",out[i]);
		else printf(" %5d ",out[i]);
	}

	/* Free mutex */
	obstaclesLocked = 0;
	/* Free global memory */
	//free( ObstaclePosData.vals );
	free(out);
}

 void myIPC_Init(char *centralhost)
//void myIPC_Init(char *centralhost)
{
	printf("\n *** Trying to connect to host \"%s\"\n\n", centralhost);

	errMsg = IPC_connectModule( MODULE_NAME, centralhost );
	printErrMsg("IPC_connectModule errMsg", errMsg);

	/* Subscribe to the messages that this module listens to. */
	errMsg = IPC_subscribe( POSE_MSG, poseMsgHandler, (void*)MODULE_NAME );
	printErrMsg("IPC_subscribe errMsg    ", errMsg);

	errMsg = IPC_subscribe( OBSTACLE_MSG, obstacleMsgHandler, (void*)MODULE_NAME );
	printErrMsg("IPC_subscribe errMsg    ", errMsg);

	/* Set message queue length */
	errMsg = IPC_setMsgQueueLength( POSE_MSG, QUEUE_LENGTH );  
	printErrMsg("setMsgQueueLength errMsg", errMsg);

	errMsg = IPC_setMsgQueueLength( OBSTACLE_MSG, QUEUE_LENGTH );
	printErrMsg("setMsgQueueLength errMsg", errMsg);

}



void myShutdown()
{
	//printf("before Disconnected from Central Server.\n");
	printf("\n *** Trying to disconnect \"%s\"\n\n", MODULE_NAME);
	errMsg = IPC_disconnect();

	printErrMsg("IPC_disconnect errMsg: ", errMsg);
	//printf("IPC_disconnect errMsg: %d\n", errMsg);
	//printf("Disconnected from Central Server.\n");
}


